from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import PathJoinSubstitution
from launch_ros.substitutions import FindPackageShare


def generate_launch_description():
    """
    实车相关控制节点
    """

    controller = Node(
        package="controller",
        executable="tri_pid",
        name="controller",
        output="screen",
        parameters=[
            {"gyro_pid": [0.6, 0.1, 0.0, 0.5]},  # 0.5
            {"angle_pid": [3.0, 0.1, 0.0, 5.0]},  # 3.0  0.1
            {"speed_pid": [30.0, 10.0, 0.0, 0.1]},  # 1  3
            {"rotate_pid": [0.5, 0.2, 0.0, 20.0]},  # 0.5 0.2
            {"gyro_angle_forward": 0.5},  # 0.8
            {"max_angle": 5.0},
            {"max_gyro": 10.0},
            {"max_speed": 1.0},
            {"gyro_filter": 0.8},
            {"angle_filter": 0.8},
            {"speed_filter": 0.95},
            {"gyroz_filter": 0.2},
            {"angle_offset": 1.0},
            {"state_rate": 100.0},
        ],
    )

    chassis = Node(
        package="chassis",
        executable="chassis_server",
        name="chassis_server",
        output="screen",
        parameters=[
            {"force_scale": 500.0},
            {"speed_limit": 1.0},
        ],
    )

    odom_node = Node(
        package="controller",
        executable="odom",
        name="real_odom",
        output="screen",
    )
    connector = Node(
        package="chassis",
        executable="connection_control",
        name="connection",
        output="screen",
    )

    return LaunchDescription(
        [
            chassis,
            # connector,
            # odom_node,
            controller,
        ]
    )
